#include <windows.h>
#include "gl\gl.h"
#include "math.h"
#include "frustum.h"

void Frustum::calculateFrustum()
{    
	float   projectionMatrix[16];
	float   modelViewMatrix[16];
	float   clippingPlanes[16];

	glGetFloatv(GL_PROJECTION_MATRIX, projectionMatrix);
	glGetFloatv(GL_MODELVIEW_MATRIX, modelViewMatrix);

	clippingPlanes[ 0] = modelViewMatrix[ 0] * projectionMatrix[ 0] + modelViewMatrix[ 1] * projectionMatrix[ 4] + modelViewMatrix[ 2] * projectionMatrix[ 8] + modelViewMatrix[ 3] * projectionMatrix[12];
	clippingPlanes[ 1] = modelViewMatrix[ 0] * projectionMatrix[ 1] + modelViewMatrix[ 1] * projectionMatrix[ 5] + modelViewMatrix[ 2] * projectionMatrix[ 9] + modelViewMatrix[ 3] * projectionMatrix[13];
	clippingPlanes[ 2] = modelViewMatrix[ 0] * projectionMatrix[ 2] + modelViewMatrix[ 1] * projectionMatrix[ 6] + modelViewMatrix[ 2] * projectionMatrix[10] + modelViewMatrix[ 3] * projectionMatrix[14];
	clippingPlanes[ 3] = modelViewMatrix[ 0] * projectionMatrix[ 3] + modelViewMatrix[ 1] * projectionMatrix[ 7] + modelViewMatrix[ 2] * projectionMatrix[11] + modelViewMatrix[ 3] * projectionMatrix[15];

	clippingPlanes[ 4] = modelViewMatrix[ 4] * projectionMatrix[ 0] + modelViewMatrix[ 5] * projectionMatrix[ 4] + modelViewMatrix[ 6] * projectionMatrix[ 8] + modelViewMatrix[ 7] * projectionMatrix[12];
	clippingPlanes[ 5] = modelViewMatrix[ 4] * projectionMatrix[ 1] + modelViewMatrix[ 5] * projectionMatrix[ 5] + modelViewMatrix[ 6] * projectionMatrix[ 9] + modelViewMatrix[ 7] * projectionMatrix[13];
	clippingPlanes[ 6] = modelViewMatrix[ 4] * projectionMatrix[ 2] + modelViewMatrix[ 5] * projectionMatrix[ 6] + modelViewMatrix[ 6] * projectionMatrix[10] + modelViewMatrix[ 7] * projectionMatrix[14];
	clippingPlanes[ 7] = modelViewMatrix[ 4] * projectionMatrix[ 3] + modelViewMatrix[ 5] * projectionMatrix[ 7] + modelViewMatrix[ 6] * projectionMatrix[11] + modelViewMatrix[ 7] * projectionMatrix[15];

	clippingPlanes[ 8] = modelViewMatrix[ 8] * projectionMatrix[ 0] + modelViewMatrix[ 9] * projectionMatrix[ 4] + modelViewMatrix[10] * projectionMatrix[ 8] + modelViewMatrix[11] * projectionMatrix[12];
	clippingPlanes[ 9] = modelViewMatrix[ 8] * projectionMatrix[ 1] + modelViewMatrix[ 9] * projectionMatrix[ 5] + modelViewMatrix[10] * projectionMatrix[ 9] + modelViewMatrix[11] * projectionMatrix[13];
	clippingPlanes[10] = modelViewMatrix[ 8] * projectionMatrix[ 2] + modelViewMatrix[ 9] * projectionMatrix[ 6] + modelViewMatrix[10] * projectionMatrix[10] + modelViewMatrix[11] * projectionMatrix[14];
	clippingPlanes[11] = modelViewMatrix[ 8] * projectionMatrix[ 3] + modelViewMatrix[ 9] * projectionMatrix[ 7] + modelViewMatrix[10] * projectionMatrix[11] + modelViewMatrix[11] * projectionMatrix[15];

	clippingPlanes[12] = modelViewMatrix[12] * projectionMatrix[ 0] + modelViewMatrix[13] * projectionMatrix[ 4] + modelViewMatrix[14] * projectionMatrix[ 8] + modelViewMatrix[15] * projectionMatrix[12];
	clippingPlanes[13] = modelViewMatrix[12] * projectionMatrix[ 1] + modelViewMatrix[13] * projectionMatrix[ 5] + modelViewMatrix[14] * projectionMatrix[ 9] + modelViewMatrix[15] * projectionMatrix[13];
	clippingPlanes[14] = modelViewMatrix[12] * projectionMatrix[ 2] + modelViewMatrix[13] * projectionMatrix[ 6] + modelViewMatrix[14] * projectionMatrix[10] + modelViewMatrix[15] * projectionMatrix[14];
	clippingPlanes[15] = modelViewMatrix[12] * projectionMatrix[ 3] + modelViewMatrix[13] * projectionMatrix[ 7] + modelViewMatrix[14] * projectionMatrix[11] + modelViewMatrix[15] * projectionMatrix[15];
	
	frustum[RIGHT][A] = clippingPlanes[ 3] - clippingPlanes[ 0];
	frustum[RIGHT][B] = clippingPlanes[ 7] - clippingPlanes[ 4];
	frustum[RIGHT][C] = clippingPlanes[11] - clippingPlanes[ 8];
	frustum[RIGHT][D] = clippingPlanes[15] - clippingPlanes[12];

	normalizePlane(frustum, RIGHT);

	frustum[LEFT][A] = clippingPlanes[ 3] + clippingPlanes[ 0];
	frustum[LEFT][B] = clippingPlanes[ 7] + clippingPlanes[ 4];
	frustum[LEFT][C] = clippingPlanes[11] + clippingPlanes[ 8];
	frustum[LEFT][D] = clippingPlanes[15] + clippingPlanes[12];

	normalizePlane(frustum, LEFT);

	frustum[BOTTOM][A] = clippingPlanes[ 3] + clippingPlanes[ 1];
	frustum[BOTTOM][B] = clippingPlanes[ 7] + clippingPlanes[ 5];
	frustum[BOTTOM][C] = clippingPlanes[11] + clippingPlanes[ 9];
	frustum[BOTTOM][D] = clippingPlanes[15] + clippingPlanes[13];

	normalizePlane(frustum, BOTTOM);

	frustum[TOP][A] = clippingPlanes[ 3] - clippingPlanes[ 1];
	frustum[TOP][B] = clippingPlanes[ 7] - clippingPlanes[ 5];
	frustum[TOP][C] = clippingPlanes[11] - clippingPlanes[ 9];
	frustum[TOP][D] = clippingPlanes[15] - clippingPlanes[13];

	normalizePlane(frustum, TOP);

	frustum[BACK][A] = clippingPlanes[ 3] - clippingPlanes[ 2];
	frustum[BACK][B] = clippingPlanes[ 7] - clippingPlanes[ 6];
	frustum[BACK][C] = clippingPlanes[11] - clippingPlanes[10];
	frustum[BACK][D] = clippingPlanes[15] - clippingPlanes[14];

	normalizePlane(frustum, BACK);

	frustum[FRONT][A] = clippingPlanes[ 3] + clippingPlanes[ 2];
	frustum[FRONT][B] = clippingPlanes[ 7] + clippingPlanes[ 6];
	frustum[FRONT][C] = clippingPlanes[11] + clippingPlanes[10];
	frustum[FRONT][D] = clippingPlanes[15] + clippingPlanes[14];

	normalizePlane(frustum, FRONT);
}

void Frustum::normalizePlane(float frustum[6][4], int side)
{
	float magnitude = (float)sqrt( frustum[side][A] * frustum[side][A] + 
								   frustum[side][B] * frustum[side][B] + 
								   frustum[side][C] * frustum[side][C] );

	frustum[side][A] /= magnitude;
	frustum[side][B] /= magnitude;
	frustum[side][C] /= magnitude;
	frustum[side][D] /= magnitude; 
}

bool Frustum::pointInFrustum(float x, float y, float z)
{
	for(int i = 0; i < 6; i++)
	{
		if(frustum[i][A] * x + frustum[i][B] * y + frustum[i][C] * z + frustum[i][D] <= 0)
		{
			// the point was behind a side, so it isn't in the frustum
			return false;
		}
	}

	// the point was inside of the frustum
	return true;
}

bool Frustum::sphereInFrustum(float x, float y, float z, float radius)
{
	for(int i = 0; i < 6; i++)	
	{
		if(frustum[i][A] * x + frustum[i][B] * y + frustum[i][C] * z + frustum[i][D] <= -radius)
		{
			return false;
		}
	}
	
	return true;
}

bool Frustum::cubeInFrustum(float x, float y, float z, float size)
{
	for(int i = 0; i < 6; i++)
	{
		if(frustum[i][A] * (x - size) + frustum[i][B] * (y - size) + frustum[i][C] * (z - size) + frustum[i][D] > 0)
		   continue;
		if(frustum[i][A] * (x + size) + frustum[i][B] * (y - size) + frustum[i][C] * (z - size) + frustum[i][D] > 0)
		   continue;
		if(frustum[i][A] * (x - size) + frustum[i][B] * (y + size) + frustum[i][C] * (z - size) + frustum[i][D] > 0)
		   continue;
		if(frustum[i][A] * (x + size) + frustum[i][B] * (y + size) + frustum[i][C] * (z - size) + frustum[i][D] > 0)
		   continue;
		if(frustum[i][A] * (x - size) + frustum[i][B] * (y - size) + frustum[i][C] * (z + size) + frustum[i][D] > 0)
		   continue;
		if(frustum[i][A] * (x + size) + frustum[i][B] * (y - size) + frustum[i][C] * (z + size) + frustum[i][D] > 0)
		   continue;
		if(frustum[i][A] * (x - size) + frustum[i][B] * (y + size) + frustum[i][C] * (z + size) + frustum[i][D] > 0)
		   continue;
		if(frustum[i][A] * (x + size) + frustum[i][B] * (y + size) + frustum[i][C] * (z + size) + frustum[i][D] > 0)
		   continue;

		return false;
	}

	return true;
}

bool Frustum::boxInFrustum(float x, float y, float z, float x2, float y2, float z2)
{
	for(int i = 0; i < 6; i++)
	{
		if(frustum[i][A] * x  + frustum[i][B] * y  + frustum[i][C] * z  + frustum[i][D] > 0)  continue;
		if(frustum[i][A] * x2 + frustum[i][B] * y  + frustum[i][C] * z  + frustum[i][D] > 0)  continue;
		if(frustum[i][A] * x  + frustum[i][B] * y2 + frustum[i][C] * z  + frustum[i][D] > 0)  continue;
		if(frustum[i][A] * x2 + frustum[i][B] * y2 + frustum[i][C] * z  + frustum[i][D] > 0)  continue;
		if(frustum[i][A] * x  + frustum[i][B] * y  + frustum[i][C] * z2 + frustum[i][D] > 0)  continue;
		if(frustum[i][A] * x2 + frustum[i][B] * y  + frustum[i][C] * z2 + frustum[i][D] > 0)  continue;
		if(frustum[i][A] * x  + frustum[i][B] * y2 + frustum[i][C] * z2 + frustum[i][D] > 0)  continue;
		if(frustum[i][A] * x2 + frustum[i][B] * y2 + frustum[i][C] * z2 + frustum[i][D] > 0)  continue;

		return false;
	}

	return true;
}